#include "pid.h"

void pid_init(pid_TypeDef* hpid, float Kp, float Ki, float Kd, float f, float integral_limit, int mode)			//这个函数的作用就是给你的电机舵机结构体内部的系数赋值
{
	hpid ->Kp = Kp;					//比例系数
	hpid ->Ki = Ki;					//积分系数
	hpid ->Kd  = Kd;				//微分系数
	f = (f == 0) ? 1 : f;		//积分和微分的标度因子，用来调节积分和微分的影响权重，且不可为零
	hpid ->f = f;
	hpid ->integral_limit = integral_limit;		//积分限幅的幅度
	hpid ->mode = mode;			//0表示位置式PID，1代表增量式PID
}

float pid_calculate(pid_TypeDef* hpid, float error)					//PID计算函数
{
	if (hpid->mode == 0)		//0代表位置式PID
	{
		hpid ->error_integral += error ;													//离散积分累加项
	
		if (hpid->error_integral > hpid->integral_limit) {				//积分限幅(非归一化）注意积分限幅需要使用原始数据，需要和output同单位

				hpid->error_integral = hpid->integral_limit;
		}
		if (hpid->error_integral < -hpid->integral_limit) {
				hpid->error_integral = -hpid->integral_limit;
		}
	
		float output = error * hpid -> Kp																			//PID公式，具体网上查
								 + hpid -> error_integral * hpid -> Ki / hpid ->f
						     + (error - hpid ->error_last) * hpid -> Kd * hpid -> f;
		hpid->error_last = error;																							//离散微分需要使用的后向差分项
	
		if(output > 1) output = 1;									//PID输出限幅
		if(output < -1) output = -1;
		return  output ; 
	} 
	else 			//增量式不会搞
	{
		float output = hpid ->Kp * (error - hpid ->error_last)
								 + hpid ->Ki * error / hpid ->f
								 + hpid ->Kd * (error - 2 * hpid ->error_last + hpid ->error_last_last) *hpid ->f;
		hpid ->error_last_last = hpid ->error_last;
		hpid ->error_last = error;
		
		hpid ->output_accumulation += output ;
		if(hpid ->output_accumulation > 1) hpid ->output_accumulation = 1;									//PID输出限幅
		if(hpid ->output_accumulation < -1) hpid ->output_accumulation = -1;
		return hpid ->output_accumulation;
	}
}

